'''包含蓝牙，循迹两种模式。蓝牙模式可以控制设备的麦轮行走，夹具的夹取与松开。循迹模式用来自动循迹完成指定功能。'''
from machine import SoftI2C
from motor import Motors
from pyb import Pin,delay
import MyFixture as Fixture
from pyb import UART
from MySensor import FrontBackCheckRun1,SlowStop
import pyb_message

raspi=UART(2,9600)#TX:A2、RX:A3

# 多功能电机模块I2C端口
i2c1=SoftI2C(sda=Pin("B11"), scl=Pin("B10"))
motors=Motors(i2c1)
mode = '1'  # 1: 表示蓝牙模式 2: 循迹模式
TrackSpeed=60
def Go(Speed):  # ++++ 前进
    Speed=int(Speed*40.95)
    motors.speed(0,Speed) #直流电机0正转，速度0~4095,4095表示最快速度
    motors.speed(1,Speed)
    motors.speed(2,Speed)
    motors.speed(3,Speed)
    print('Go')

def Back(Speed):# ---- 后退
    Speed=int(Speed*40.95)
    motors.speed(0,-Speed) #直流电机0反转，速度0~-4095,-4095表示反转最快速度
    motors.speed(1,-Speed)
    motors.speed(2,-Speed)
    motors.speed(3,-Speed)
    print('Back')

def LeftGo(Speed): # +-+- 左走
    Speed=int(Speed*40.95)
    motors.speed(0,Speed) #直流电机0反转，速度0~-4095,-4095表示反转最快速度
    motors.speed(1,-Speed)
    motors.speed(2,Speed)
    motors.speed(3,-Speed)
    print('LeftGo')

def RightGo(Speed):  # -+-+ 右走
    Speed=int(Speed*40.95)
    motors.speed(0,-Speed) #直流电机0反转，速度0~-4095,-4095表示反转最快速度
    motors.speed(1,Speed)
    motors.speed(2,-Speed)
    motors.speed(3,Speed)
    print('RightGO')

def TurnRight(Speed):  # -++- 左转
    Speed=int(Speed*40.95)
    motors.speed(0,-Speed) #直流电机0反转，速度0~-4095,-4095表示反转最快速度
    motors.speed(1,Speed)
    motors.speed(2,Speed)
    motors.speed(3,-Speed)
    print('TurnRight')

def TurnLeft(Speed): # +--+ 右转
    Speed=int(Speed*40.95)
    motors.speed(0,Speed) #直流电机0反转，速度0~-4095,-4095表示反转最快速度
    motors.speed(1,-Speed)
    motors.speed(2,-Speed)
    motors.speed(3,Speed)
    print('TurnLeft')

def LeftGo45(Speed):  # +0+0 左前45°
    Speed=int(Speed*40.95)
    motors.speed(0,Speed) #直流电机0反转，速度0~-4095,-4095表示反转最快速度
    motors.speed(1,0)
    motors.speed(2,Speed)
    motors.speed(3,0)
    print('LeftGo45')

def RightGo45(Speed):  # 0+0+ 右前45°
    Speed=int(Speed*40.95)
    motors.speed(0,0) #直流电机0反转，速度0~-4095,-4095表示反转最快速度
    motors.speed(1,Speed)
    motors.speed(2,0)
    motors.speed(3,Speed)
    print('RightGo45')

def LeftBack45(Speed):  # 0-0- 左后45°
    Speed=int(Speed*40.95)
    motors.speed(0,0) #直流电机0反转，速度0~-4095,-4095表示反转最快速度
    motors.speed(1,-Speed)
    motors.speed(2,0)
    motors.speed(3,-Speed)
    print('LeftBack45')

def RightBack45(Speed): # -0-0 右后45°
    Speed=int(Speed*40.95)
    motors.speed(0,-Speed) #直流电机0反转，速度0~-4095,-4095表示反转最快速度
    motors.speed(1,0)
    motors.speed(2,-Speed)
    motors.speed(3,0)
    print('RightBack45')

def HardStop():
    #制动停止
    for i in range(4):
        motors.brake(i)
    print('HardStop')

def SoftStop():
    motors.speed(0,0) #直流电机0反转，速度0~-4095,-4095表示反转最快速度
    motors.speed(1,0)
    motors.speed(2,0)
    motors.speed(3,0)
    print('SoftStop')
boxnum=0
#蓝牙部分
def bluetooth(BLE):
    global mode
    global box
    print('lanya')
    while 1:
        if BLE.uart.any():  # 查询是否有信息
            text = BLE.uart.read(1)  # 默认单次最多接收128字节
            if text.find(b'o')>-1:#停机
                HardStop()
                mode='3'
                return 3
            elif text.find(b's')>-1:
                HardStop()
                mode="2"#循迹
                return 2
            elif text.find(b'c')>-1:
                HardStop()
            #夹具夹紧松开
            elif text.find(b'1')>-1:#夹取
                Fixture.TestTake()
            elif text.find(b'2')>-1:#松开
                Fixture.TestPut()
            elif text.find(b'3')>-1:#夹具停
                Fixture.Stop()

            #套箱升降
            elif text.find(b'4')>-1:#夹取
                Fixture.TestUp()
                boxnum=1
            elif text.find(b'5')>-1:#松开
                Fixture.TestDown()
                boxnum=2
            elif text.find(b'6')>-1:#夹具停
                if boxnum==1:
                    Fixture.TestDown()
                    delay(10)
                elif boxnum==2:
                    Fixture.TestUp()
                    delay(10)
                Fixture.TestUpDownStop()
                boxnum=0

            #向前
            elif text.find(b'g')>-1:
                Go(100)
            #后退
            elif text.find(b'b') >-1:
                Back(100)
            #左走
            elif text.find( b'l')>-1:
                LeftGo(100)
            #右走
            elif text.find( b'r')>-1:
                RightGo(100)
            #左转
            elif text.find( b'm')>-1:
                TurnLeft(100)
            #右转
            elif text.find(b'n')>-1:
                TurnRight(100)
            #左前45
            elif text.find(b'h')>-1:
                LeftGo45(100)
            #右前45
            elif text.find( b'i')>-1:
                RightGo45(100)
            #左后45
            elif text.find( b'j')>-1:
                LeftBack45(100)
            #右后45
            elif text.find( b'k')>-1:
                RightBack45(100)
'''循迹部分'''
def HowRun(lrg,RunTime):
    if lrg=='g':
        print('Go')
        Go(TrackSpeed)
        delay(RunTime)
    if lrg=='b':
        print('Back')
        Back(TrackSpeed)
        delay(RunTime)
    if lrg=='c':
        print('Stop')
        HardStop()
    elif lrg=='l':
        print('LeftGo')
        LeftGo(TrackSpeed)
        delay(RunTime)
    elif lrg=='m':
        print('TurnLeft')
        TurnLeft(TrackSpeed)
        delay(RunTime)
    elif lrg=='r':
        print('RightGo')
        RightGo(TrackSpeed)
        delay(RunTime)
    elif lrg=='n':
        print('TurnRight')
        TurnRight(TrackSpeed)
        delay(RunTime)
    #左前45
    elif lrg=='h':
        print('LeftGo45')
        LeftGo45(TrackSpeed)
        delay(RunTime)
    #右前45
    elif lrg=='i':
        print('RightGo45')
        RightGo45(TrackSpeed)
        delay(RunTime)
    #左后45
    elif lrg=='j':
        print('LeftBack45')
        LeftBack45(TrackSpeed)
        delay(RunTime)
    #右后45
    elif lrg=='k':
        print('RightBack45')
        RightBack45(TrackSpeed)
        delay(RunTime)
    else:
        Go(TrackSpeed)
        delay(50)
        print('ElseGo')


def Tracking(BLE):
    global mode
    global TrackSpeed
    print('Tracking')
    while 1:
        if BLE.uart.any():  # 查询是否有信息
            text = BLE.uart.read(1)  # 默认单次最多接收128字节
            if text.find( b'o')>-1:
                HardStop()
                mode='3'
                return 3
            elif text.find(b'a')>-1:
                HardStop()
                mode="1"#蓝牙
                return 1
        else:
            text = pyb_message.receive_text()
            if text == 'go_ahead':
                lrgt=FrontBackCheckRun1()
                lrg=lrgt[0]   #取前后循迹模块信号值做出下一步运动的判断
                RunTime=lrgt[1]   #取前后循迹模块信号值做出下一步运动的判断
                ss=SlowStop()#根据左面循迹模块判断是否停下或减速
                print(ss)
                if ss=='s':
                    print('Slow')
                    TrackSpeed=30
                    HowRun(lrg,RunTime)
                    delay(50)
                elif ss=='f':
                    print('Fast')
                    TrackSpeed=60
                    HowRun(lrg,RunTime)
                    delay(50)
                elif ss=='c':
                    print('Stop')
                    HardStop()
                    pyb_message.send_text('stop')
                    delay(50)
                else:
                    pass

def RaspiTracking(BLE):
    global mode
    global TrackSpeed
    print('Tracking')
    while 1:
        if BLE.uart.any():  # 查询是否有信息
            text = BLE.uart.read(1)  # 默认单次最多接收128字节
            if text.find(b'o') > -1:
                HardStop()
                mode = '3'
                return 3
            elif text.find(b'a') > -1:
                HardStop()
                mode = "1"  # 蓝牙
                return 1
        else:
            text = pyb_message.receive_text()
            if text == 'go_ahead':
                lrgt = FrontBackCheckRun1()
                lrg = lrgt[0]  # 取前后循迹模块信号值做出下一步运动的判断
                RunTime = lrgt[1]  # 取前后循迹模块信号值做出下一步运动的判断
                ss = SlowStop()  # 根据左面循迹模块判断是否停下或减速
                print(ss)
                if ss == 's':
                    print('Slow')
                    TrackSpeed = 30
                    HowRun(lrg, RunTime)
                    delay(50)
                elif ss == 'f':
                    print('Fast')
                    TrackSpeed = 60
                    HowRun(lrg, RunTime)
                    delay(50)
                elif ss == 'c':
                    print('Stop')
                    HardStop()
                    pyb_message.send_text('stop')
                    delay(50)
                else:
                    pass

def TestHowRun(lrg):
    if lrg=='g':
        print('Go')
        Go(TrackSpeed)
    if lrg=='b':
        print('Back')
        Back(TrackSpeed)
    if lrg=='c':
        print('Stop')
        HardStop()
    elif lrg=='l':
        print('LeftGo')
        LeftGo(TrackSpeed)
    elif lrg=='m':
        print('TurnLeft')
        TurnLeft(TrackSpeed)
    elif lrg=='r':
        print('RightGo')
        RightGo(TrackSpeed)
    elif lrg=='n':
        print('TurnRight')
        TurnRight(TrackSpeed)
    #左前45
    elif lrg=='h':
        print('LeftGo45')
        LeftGo45(TrackSpeed)
    #右前45
    elif lrg=='i':
        print('RightGo45')
        RightGo45(TrackSpeed)
    #左后45
    elif lrg=='j':
        print('LeftBack45')
        LeftBack45(TrackSpeed)
    #右后45
    elif lrg=='k':
        print('RightBack45')
        RightBack45(TrackSpeed)
    else:
        print('ElseGo')
        Go(TrackSpeed)


if __name__=="__main__":
    while 1:
        if mode=="3":#停机
            SoftStop()
            break
        if mode=="1":#蓝牙
            bluetooth()
        elif mode=="2":#循迹
            Tracking()
        elif mode=="4":#树莓派控制
            RaspiTracking()
        else:
            pass
